import differential as p1
import numpy as np
import matplotlib.pylab as mp
from math import sqrt

## -------------------------  pendule simple  ------------------------------##
## equation :
## y'' + W^2sin(y) = 0 si y petit y'' + W^2y = 0 avec W^2=g/l


d = np.pi / 2
step = 0.001
N = int (d / step)
# tige de longueur  10cm
l=0.1
g=9.81

y0 = np.array([1.5, 0.])
t0 = np.array(0)

f_expe = lambda y, t: np.array([y[1], (-g/l)*np.sin(y[0])])
f_theo = lambda y, t: y0[0]*np.cos((sqrt(g/l)*t))


# resolution de l'equation differentielle du pendule a l'aide des methodes mises en place dans la partie 1
def pendule_plot ():

	T_theo = p1.meth_N_step_theo(t0, N, step, f_theo)
	V = p1.meth_N_step(y0, t0, N, step, f_expe, p1.step_rk4)
	T_expe = [0 for i in range(0, np.shape(V)[0])]
	for i in range (0, np.shape(V)[0]):
		T_expe[i] = V[i][0]

	plt1, = mp.plot(T_expe)
	plt2, = mp.plot(T_theo)
	mp.xlabel("t")
	mp.ylabel("theta")
	mp.legend([plt1, plt2], ["courbe experimentale", "courbe theorique"])
	# mp.savefig("pendule_simple")
	mp.show()


## frequence ##

# returns the frequency of the function or -1 if the funtion is not periodical
def function_freq (T, epsi):
	i = 0
	j = 1
	length = len(T)-1

	while j < length and (abs(T[j] - T[i]) >= epsi or (T[j+1]-T[j]) * (T[i+1]-T[i]) > 0):
		j+=1
	
	if j == length:
		return -1
	else :
		while j < length and abs(T[j] - T[i]) <= 2*epsi:
			i+=1
			j+=1
		if j == length:
			return float(2*np.pi / ((j - i) * step))
		else :
			return -1


def pendule_freq (theta0):

	V = p1.meth_N_step(theta0, t0, N, step, f_expe, p1.step_rk4)
	T_expe = [0 for i in range(0, np.shape(V)[0])]
	for i in range (0, np.shape(V)[0]):
		T_expe[i] = V[i][0]
	epsi = 0.001
	while(function_freq (T_expe, epsi)) == -1:
		epsi += 0.0001
	return function_freq (T_expe, epsi)



def plot_pendule_freq ():
	theta = np.arange(-np.pi/4., np.pi/4., 0.1)
	T = []
	for i in range(len(theta)):
		T = T + [pendule_freq([theta[i], 0])]
	mp.plot(theta, T)

	mp.xlabel("Angle initial")
	mp.ylabel("Frequence du pendule")
	# mp.savefig("pendule_frequence")
	mp.show()



pendule_plot ()
plot_pendule_freq ()